From 59b394d0d2b2e11687e757820c52d6470d15a9c5 Mon Sep 17 00:00:00 2001
From: Christian Lamparter <chunkeey@gmail.com>
Date: Mon, 13 Jun 2016 15:42:21 +0200
Subject: [PATCH] phy device tree support for emac

---
 drivers/net/ethernet/ibm/emac/core.c | 261 +++++++++++++++++++++++++++++++++++
 drivers/net/ethernet/ibm/emac/core.h |   4 +
 2 files changed, 265 insertions(+)

--- a/drivers/net/ethernet/ibm/emac/core.c
+++ b/drivers/net/ethernet/ibm/emac/core.c
@@ -42,6 +42,7 @@
 #include <linux/of_address.h>
 #include <linux/of_irq.h>
 #include <linux/of_net.h>
+#include <linux/of_mdio.h>
 #include <linux/slab.h>
 
 #include <asm/processor.h>
@@ -2410,6 +2411,246 @@ static int emac_read_uint_prop(struct de
 	return 0;
 }
 
+static void emac_adjust_link(struct net_device *ndev)
+{
+	struct emac_instance *dev = netdev_priv(ndev);
+	struct phy_device *phy = dev->phy_dev;
+
+	dev->phy.autoneg = phy->autoneg;
+	dev->phy.speed = phy->speed;
+	dev->phy.duplex = phy->duplex;
+	dev->phy.pause = phy->pause;
+	dev->phy.asym_pause = phy->asym_pause;
+	dev->phy.advertising = phy->advertising;
+}
+
+static int emac_mii_bus_read(struct mii_bus *bus, int addr, int regnum)
+{
+	return emac_mdio_read(bus->priv, addr, regnum);
+}
+
+static int emac_mii_bus_write(struct mii_bus *bus, int addr, int regnum, u16 val)
+{
+	emac_mdio_write(bus->priv, addr, regnum, val);
+	return 0;
+}
+
+static int emac_mii_bus_reset(struct mii_bus *bus)
+{
+	struct emac_instance *dev = netdev_priv(bus->priv);
+
+	emac_mii_reset_phy(&dev->phy);
+	return 0;
+}
+
+static int emac_mdio_setup_aneg(struct mii_phy *phy, u32 advertise)
+{
+	struct net_device *ndev = phy->dev;
+	struct emac_instance *dev = netdev_priv(ndev);
+
+	dev->phy.autoneg = AUTONEG_ENABLE;
+	dev->phy.speed = SPEED_1000;
+	dev->phy.duplex = DUPLEX_FULL;
+	dev->phy.advertising = advertise;
+	phy->autoneg = AUTONEG_ENABLE;
+	phy->speed = dev->phy.speed;
+	phy->duplex = dev->phy.duplex;
+	phy->advertising = advertise;
+	return phy_start_aneg(dev->phy_dev);
+}
+
+static int emac_mdio_setup_forced(struct mii_phy *phy, int speed, int fd)
+{
+	struct net_device *ndev = phy->dev;
+	struct emac_instance *dev = netdev_priv(ndev);
+
+	dev->phy.autoneg =  AUTONEG_DISABLE;
+	dev->phy.speed = speed;
+	dev->phy.duplex = fd;
+	phy->autoneg = AUTONEG_DISABLE;
+	phy->speed = speed;
+	phy->duplex = fd;
+	return phy_start_aneg(dev->phy_dev);
+}
+
+static int emac_mdio_poll_link(struct mii_phy *phy)
+{
+	struct net_device *ndev = phy->dev;
+	struct emac_instance *dev = netdev_priv(ndev);
+	int res;
+
+	res = phy_read_status(dev->phy_dev);
+	if (res) {
+		dev_err(&dev->ndev->dev, "link update failed (%d).", res);
+		return ethtool_op_get_link(ndev);
+	}
+
+	return dev->phy_dev->link;
+}
+
+static int emac_mdio_read_link(struct mii_phy *phy)
+{
+	struct net_device *ndev = phy->dev;
+	struct emac_instance *dev = netdev_priv(ndev);
+	int res;
+
+	res = phy_read_status(dev->phy_dev);
+	if (res)
+		return res;
+
+	dev->phy.speed = phy->speed;
+	dev->phy.duplex = phy->duplex;
+	dev->phy.pause = phy->pause;
+	dev->phy.asym_pause = phy->asym_pause;
+	return 0;
+}
+
+static int emac_mdio_init_phy(struct mii_phy *phy)
+{
+	struct net_device *ndev = phy->dev;
+	struct emac_instance *dev = netdev_priv(ndev);
+
+	phy_start(dev->phy_dev);
+	dev->phy.autoneg = phy->autoneg;
+	dev->phy.speed = phy->speed;
+	dev->phy.duplex = phy->duplex;
+	dev->phy.advertising = phy->advertising;
+	dev->phy.pause = phy->pause;
+	dev->phy.asym_pause = phy->asym_pause;
+
+	return phy_init_hw(dev->phy_dev);
+}
+
+static const struct mii_phy_ops emac_dt_mdio_phy_ops = {
+	.init		= emac_mdio_init_phy,
+	.setup_aneg	= emac_mdio_setup_aneg,
+	.setup_forced	= emac_mdio_setup_forced,
+	.poll_link	= emac_mdio_poll_link,
+	.read_link	= emac_mdio_read_link,
+};
+
+static void emac_dt_phy_mdio_cleanup(struct emac_instance *dev)
+{
+	if (dev->mii_bus) {
+		if (dev->mii_bus->state == MDIOBUS_REGISTERED)
+			mdiobus_unregister(dev->mii_bus);
+		mdiobus_free(dev->mii_bus);
+		dev->mii_bus = NULL;
+	}
+	kfree(dev->phy.def);
+	dev->phy.def = NULL;
+}
+
+static int emac_dt_mdio_probe(struct emac_instance *dev)
+{
+	struct device_node *mii_np;
+	int res;
+
+	mii_np = of_get_child_by_name(dev->ofdev->dev.of_node, "mdio");
+	if (!mii_np) {
+		dev_err(&dev->ndev->dev, "no mdio definition found.");
+		return -ENODEV;
+	}
+
+	if (!of_device_is_available(mii_np)) {
+		res = 1;
+		goto err_put_node;
+	}
+
+	dev->mii_bus = mdiobus_alloc();
+	if (!dev->mii_bus) {
+		res = -ENOMEM;
+		goto err_cleanup_mdio;
+	}
+
+	dev->mii_bus->priv = dev->ndev;
+	dev->mii_bus->parent = dev->ndev->dev.parent;
+	dev->mii_bus->name = "emac_mdio";
+	dev->mii_bus->read = &emac_mii_bus_read;
+	dev->mii_bus->write = &emac_mii_bus_write;
+	dev->mii_bus->reset = &emac_mii_bus_reset;
+	snprintf(dev->mii_bus->id, MII_BUS_ID_SIZE, "%s", dev->mii_bus->name);
+
+	res = of_mdiobus_register(dev->mii_bus, mii_np);
+	if (res) {
+		dev_err(&dev->ndev->dev, "cannot register MDIO bus %s (%d)",
+			dev->mii_bus->name, res);
+		goto err_cleanup_mdio;
+	}
+	of_node_put(mii_np);
+	return 0;
+
+ err_cleanup_mdio:
+	emac_dt_phy_mdio_cleanup(dev);
+ err_put_node:
+	of_node_put(mii_np);
+	return res;
+}
+
+static int emac_dt_phy_probe(struct emac_instance *dev,
+			     struct device_node *phy_handle)
+{
+	u32 phy_flags = 0;
+	int res;
+
+	res = of_property_read_u32(phy_handle, "phy-flags", &phy_flags);
+	if (res < 0 && res != -EINVAL)
+		return res;
+
+	dev->phy.def = kzalloc(sizeof(*dev->phy.def), GFP_KERNEL);
+	if (!dev->phy.def)
+		return -ENOMEM;
+
+	dev->phy_dev = of_phy_connect(dev->ndev, phy_handle,
+				      &emac_adjust_link, phy_flags,
+				      PHY_INTERFACE_MODE_RGMII);
+	if (!dev->phy_dev) {
+		dev_err(&dev->ndev->dev, "failed to connect to PHY.");
+		kfree(dev->phy.dev);
+		dev->phy.dev = NULL;
+		return -ENODEV;
+	}
+
+	dev->phy.def->phy_id = dev->phy_dev->drv->phy_id;
+	dev->phy.def->phy_id_mask = dev->phy_dev->drv->phy_id_mask;
+	dev->phy.def->name = dev->phy_dev->drv->name;
+	dev->phy.def->ops = &emac_dt_mdio_phy_ops;
+	dev->phy.features = dev->phy_dev->supported;
+	dev->phy.address = dev->phy_dev->addr;
+	dev->phy.mode = dev->phy_dev->interface;
+	return 0;
+}
+
+static int emac_probe_dt_phy(struct emac_instance *dev)
+{
+	struct device_node *np = dev->ofdev->dev.of_node;
+	struct device_node *phy_handle;
+	int res = 0;
+
+	phy_handle = of_parse_phandle(np, "phy-handle", 0);
+
+	if (phy_handle) {
+		res = emac_dt_mdio_probe(dev);
+		if (res)
+			goto out;
+
+		res = emac_dt_phy_probe(dev, phy_handle);
+		if (res) {
+			emac_dt_phy_mdio_cleanup(dev);
+			goto out;
+		}
+
+		return 1;
+	}
+
+ out:
+	of_node_put(phy_handle);
+	/* if no phy device was specifie in the device tree, then we fallback
+	 * to the old emac_phy.c probe code for compatibility reasons.
+	 */
+	return res;
+}
+
 static int emac_init_phy(struct emac_instance *dev)
 {
 	struct device_node *np = dev->ofdev->dev.of_node;
@@ -2480,6 +2721,18 @@ static int emac_init_phy(struct emac_ins
 
 	emac_configure(dev);
 
+	if (emac_has_feature(dev, EMAC_FTR_HAS_RGMII)) {
+		int res = emac_probe_dt_phy(dev);
+
+		if (res == 1)
+			goto init_phy;
+		if (res < 0)
+			dev_err(&dev->ndev->dev, "failed to attach dt phy (%d).",
+				res);
+
+		/* continue with old code */
+	}
+
 	if (dev->phy_address != 0xffffffff)
 		phy_map = ~(1 << dev->phy_address);
 
@@ -2507,6 +2760,7 @@ static int emac_init_phy(struct emac_ins
 		return -ENXIO;
 	}
 
+ init_phy:
 	/* Init PHY */
 	if (dev->phy.def->ops->init)
 		dev->phy.def->ops->init(&dev->phy);
@@ -2925,6 +3179,8 @@ static int emac_probe(struct platform_de
 	/* I have a bad feeling about this ... */
 
  err_detach_tah:
+	emac_dt_phy_mdio_cleanup(dev);
+
 	if (emac_has_feature(dev, EMAC_FTR_HAS_TAH))
 		tah_detach(dev->tah_dev, dev->tah_port);
  err_detach_rgmii:
@@ -2975,6 +3231,11 @@ static int emac_remove(struct platform_d
 	if (emac_has_feature(dev, EMAC_FTR_HAS_ZMII))
 		zmii_detach(dev->zmii_dev, dev->zmii_port);
 
+	if (dev->phy_dev)
+		phy_disconnect(dev->phy_dev);
+
+	emac_dt_phy_mdio_cleanup(dev);
+
 	busy_phy_map &= ~(1 << dev->phy.address);
 	DBG(dev, "busy_phy_map now %#x" NL, busy_phy_map);
 
--- a/drivers/net/ethernet/ibm/emac/core.h
+++ b/drivers/net/ethernet/ibm/emac/core.h
@@ -199,6 +199,10 @@ struct emac_instance {
 	struct emac_instance		*mdio_instance;
 	struct mutex			mdio_lock;
 
+	/* Device-tree based phy configuration */
+	struct mii_bus			*mii_bus;
+	struct phy_device		*phy_dev;
+
 	/* ZMII infos if any */
 	u32				zmii_ph;
 	u32				zmii_port;
